#ifndef __GLOBAL_PATH_PLANNER_H__
#define __GLOBAL_PATH_PLANNER_H__

#include "full_cover_path_planner/full_cover_planner.h"
#include "implement_path_planner/implement_planner.h"
#include "msg_publish.h"
#include "PathPlan.h"

#include <ros/ros.h>

#include <nav_msgs/OccupancyGrid.h>

#include <v3_msgs/PathGenerateManager.h>
#include <v3_msgs/MapStatus.h>
#include <v3_msgs/MapInfo.h>
#include <cln_msgs/StringArray.h>
#include "statistics/include/TaskStat.h"
#include "statistics/include/TaskStatAdapter.h"

using namespace global_path_planner::ccpp;
using namespace global_path_planner::implement;
using namespace global_path_planner::preprocess;

class GlobalPathPlanner{
    public:
        GlobalPathPlanner();
        virtual ~GlobalPathPlanner();
    private:
        // 初始化参数
        void InitParam();
        // 订阅话题
        void MapSatusCallback(const v3_msgs::MapStatusConstPtr& msg);
        void MapGridCallback(const nav_msgs::OccupancyGridConstPtr& msg);
        void MapInfoCallback(const v3_msgs::MapInfoConstPtr& msg);
        // 参数变更
        void ParamsUpdateCallback(const cln_msgs::StringArray &msg);
        // 生成静态p2p路径
        bool getP2PPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res);
        // 生成全覆盖路径
        bool getFullCoverPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res);
        // 生成推行路径
        bool getImplementPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res);
        // 获取默认充电桩位置
        std::pair<bool, std::tuple<float, float, float>> getDefaultDockerPose();
        // 将geometry_msgs::Pose转换到tuple
        std::tuple<float, float, float> PoseConvertTuple(const geometry_msgs::Pose &pose);
        // 将充电桩的位置转化为滑行点的位置
        std::tuple<float, float, float> Docker2LandingPtn(std::tuple<float, float, float> &docker_pose,
                                                        float& land_dist, int backward);
        // 获取路径类型
        int16_t GetPathType(std::vector<v3_msgs::PathGenerateType> & path_types);
        
        // 更新__task_Stat
        void UpdateTaskStat(const v3_msgs::PathGenerateManagerRequest& req,
                            const v3_msgs::PathGenerateManagerResponse& res,
                            const bool area_clean);

    private:
        // 节点句柄
        ros::NodeHandle __nh;
        // 订阅地图状态信息
        ros::Subscriber __map_Status_Sub;
        // 订阅栅格地图信息
        ros::Subscriber __map_Grid_Sub;
        // 订阅地图信息
        ros::Subscriber __map_Info_Sub;
        // 订阅参数变更
        ros::Subscriber __params_Update_Sub;
        // 请求p2p路径
        ros::ServiceServer __p2p_Static_Serve;
        // 请求全覆盖路径
        ros::ServiceServer __full_Cover_Path_Serve;
        // 请求推行路径
        ros::ServiceServer __implement_Path_Serve;
        // 默认充电桩请求
        ros::ServiceClient __map_Info_Client;
    private:
        // 全局p2p规划
        std::shared_ptr<::common::planning::PathPlan> __path_Plan_Ptr;
        // 全覆盖路径规划
        std::shared_ptr<FullCoverPlanner> __full_Cover_Planner_Ptr;
        // 推行规划
        std::shared_ptr<ImplementPlanner> __implement_Planner_Ptr;
        // 发布消息类
        std::shared_ptr<MsgPublish> __msg_Pub_Ptr;
        // 地图预处理
        std::shared_ptr<MapProcess> __map_Proc_Ptr;
        // 地图分块
        std::shared_ptr<MapSegment> __map_Seg_Ptr;
        // 地图基础数据
        std::shared_ptr<MapData> __map_Data_Ptr;
    private:
         // 当前地图更新状态
        v3_msgs::MapStatusConstPtr __map_Status_Ptr;
        // 栅格地图
        nav_msgs::OccupancyGridConstPtr __map_Grid_Ptr;
        // 地图信息
        v3_msgs::MapInfoConstPtr __map_Info_Ptr;
    private:
        //机器人半径
        float __robot_Radius;
        // 单元分解cell缓存地址
        std::string __cache_Dir;
        // 缓存地图文件夹
        string __map_Dir;
        // 调试
        bool __debug_Mode;
        // 生成任务两条路径之间的宽度
        float __path_Width;
        // 主刷宽度
        float __brush_Length;
        // 充电桩的位置到滑行点距离
        float __landing_Distance;
        // 机器人类型
        string __robotType;
        // 清洁任务统计数据相关
        common::statistics::TaskStat __task_Stat;
};
#endif